/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Collision detector
 */

// ROS
#include "ros/ros.h"
// Messages
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "sensor_msgs/LaserScan.h"
#include "group6/safeToMoveForward.h"
// Stdlibs
#include "math.h"
// Application
#include "collisionDetector.h"
/*   Setup robot parameters.
 *
 *  @param  ..
 *
 *  @return void
 */
void setupRobot()
{
    /*
    ros::NodeHandle nh;
    nh.setParam("laser_pos_x",       -0.08);    // Laser position in X
    nh.setParam("laser_pos_y",        0.0);     // Laser position in Y
    nh.setParam("chassis_pos_x",      0.18);     // Chassis position in X
    nh.setParam("chassis_pos_y",      0.0);     // Chassis position in Y
    nh.setParam("manipulator_pos_x",  0.0);     // Manipulator position in X
    nh.setParam("manipulator_pos_y",  0.0);     // Manipulator position in Y
    nh.setParam("chassis_width",      0.42);     // Chassis width
    nh.setParam("clearance_x",        0.6);    // Clearance in X axis
    nh.setParam("clearance_y",        0.1);    // Clearance in Y axis
    */
}

/*  Default constructor.
 *
 */
CollisionDetector::CollisionDetector():
    mCollision(false)
{
    mLaserScanSubscriber =              mNode.subscribe("scan", 1000, &CollisionDetector::readScan, this);
    mSafeToMoveService =                mNode.advertiseService("safeToMoveForward", &CollisionDetector::safeToMoveForward, this);
    mObstacleApproachingPublisher =     mNode.advertise<std_msgs::Bool>("obstacleApproaching", 10);

}

/*  Main.
 *
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "collisionDetector");
    CollisionDetector instance;

    // Setup robot variables. Should be in StateMachine? /LCh
    setupRobot();

    if(!instance.readRobotParameters())
    {
        ROS_INFO("Getting parameters failed");
    }

    ros::spin();

    return 0;
}

/*  Calculates measurement points of one spin of the lidar LMS100.
 *
 *  @param  scan  Message from LMS100
 *  @return int   Number of measurements
 */
int CollisionDetector::calculateSamples(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    // Make sure values are valid
    if(scan->angle_max < scan->angle_min)
        return -1;

    // Make sure increment is > 0
    if(scan->angle_increment <= 0)
        return -1;

    int samples = (scan->angle_max - scan->angle_min) / scan->angle_increment;

    return samples + 1;
}

/*  Callback when a new message has arrived to /scan topic. Reads a laser scan and calculates
 *  whether it is safe to go.
 *
 *  @param  scan  Message from LMS100
 *  @return void
 */
void CollisionDetector::readScan(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    ros::NodeHandle nh;
    int collisionSamplesCount = 0;

    // N of samples in the whole range
    int samples = this->calculateSamples(scan);
    // N on samples in between -90...90 degrees
    int usefulSamples = (PI / scan->angle_increment) + 1;
    // If somehow lidar range is smaller than -90...90 degrees
    if(usefulSamples > samples)
    {
        usefulSamples = samples;
    }

    // Go trought all samples and only pick those which are in front of the robot.
    LaserSample* usefulData = (LaserSample*)malloc(usefulSamples * sizeof(LaserSample));
    // How many samples are rejected
    int indexOffset = (samples - usefulSamples) / 2;
    int index = 0;
    for (int i = indexOffset; i < (samples - indexOffset); ++i)
    {
        usefulData[index].angle = (i - (samples / 2)) * scan->angle_increment;
        usefulData[index].distance = scan->ranges[i];
        index++;
    }

    // Calculate collisions.
    for (int i = 0; i < usefulSamples; ++i)
    {
        double y = tan(usefulData[i].angle) * this->getLaserToCollisionX();

        // Check if this point is relevant when travelling straight.
        if(y < this->getLaserToCollisionY() && y > -this->getLaserToCollisionY())
        {
            // Skip if distance value is smaller than minimum distance reported by LMS100
            if(usefulData[i].distance < scan->range_min)
            {
                ROS_INFO("Sample skipped, distance = %f.", usefulData[i].distance);
                continue;
            }
            // ROS_INFO("%d: %f @ %f", i, cos(usefulData[i].angle) * usefulData[i].distance, usefulData[i].angle);
            // Sample is closer than safety distance.
            if(cos(usefulData[i].angle) * usefulData[i].distance < this->getLaserToCollisionX())
            {
                //ROS_WARN("COLLISION @ %f (%f < %f)", usefulData[i].angle, cos(usefulData[i].angle) * usefulData[i].distance, this->getLaserToCollisionX());
                collisionSamplesCount++;
            }
        }
        //
        else if(y > this->getLaserToCollisionY())
        {

        }
        //
        else if(y < -this->getLaserToCollisionY())
        {

        }
    }

    // Report if collision is about to happen.
    if(collisionSamplesCount > COLLISION_THRESHOLD)
    {
        ROS_WARN("Collision points %d", collisionSamplesCount);
        mCollision = true;

        std_msgs::Bool stopMsg;
        stopMsg.data = 1;
        mObstacleApproachingPublisher.publish(stopMsg);

        ROS_WARN("COLLISION");
    }
    else
    {
        mCollision = false;
    }

    free(usefulData);
}


/*  Reply if it is safe to start moving.
 *
 *  @param  req     Request /Currently ignored, Liangyu
 *  @param  res     Respond, is it safe.
 *  @return bool    
 */
bool CollisionDetector::safeToMoveForward(group6::safeToMoveForward::Request &req, group6::safeToMoveForward::Response &res)
{
    res.result = !this->collision();
    ROS_INFO("SM asked if it is safe to move: %s", res.result?"yes":"no");
    return true;
}


/*  Try to get robot paremeters from paremeter server.
 *
 *  @return bool    If all parameters are found
 */
bool CollisionDetector::readRobotParameters()
{
    bool ret = true;
    ros::NodeHandle nh;

    if(!nh.getParam("laser_pos_x", mLaserPosX))
        ret = false;
    if(!nh.getParam("laser_pos_y", mLaserPosY))
        ret = false;
    if(!nh.getParam("chassis_pos_x", mChassisPosX))
        ret = false;
    if(!nh.getParam("chassis_pos_y", mChassisPosX))
        ret = false;
    if(!nh.getParam("manipulator_pos_x", mManipulatorPosX))
        ret = false;
    if(!nh.getParam("manipulator_pos_y", mManipulatorPosY))
        ret = false;
    if(!nh.getParam("chassis_width", mChassisWidth))
        ret = false;
    if(!nh.getParam("clearance_x", mClearanceX))
        ret = false;
    if(!nh.getParam("clearance_y", mClearanceY))
        ret = false;

    mLaserToCollisionX = mManipulatorPosX + mClearanceX - mLaserPosX;
    mLaserToCollisionY = (mChassisWidth / 2) + mClearanceY;

    ROS_INFO("X: %f", this->getLaserToCollisionX());
    ROS_INFO("Y: %f", this->getLaserToCollisionY());

    return ret;
}
